/*
  BenderTestSuite
  Test suite to test functionality of Bender electronics
*/

#include <Max3421e.h>
#include <Usb.h>
#include <AndroidAccessory.h>

#include <Bender.h>

AndroidAccessory acc("SOMA",
                    "Bender",
                    "Bender Arduino Board",
                    "1.0",
                    "http://code.google.com/p/vader-robot/",
                    "0000000012345678");
void setup() {
    // open the serial port:
  Serial.begin(115200);

  initBender();
  randomSeed(analogRead(0));

  acc.powerOn();
}

#define STATE_IDLE      0
#define STATE_TURN      1
#define STATE_PAUSE     2
#define STATE_TURN_PAST 3

#define HALL_OFFSET 2

int ledState[8] = {HIGH,  LOW,  LOW,  LOW,  LOW,  LOW,  LOW,  LOW};

void ledLoop(void) {
  static unsigned char renderLed = 0;
  static int state = STATE_IDLE;

  unsigned long tickNow = millis();
  turnOnLED(renderLed, ledState[renderLed]);
  renderLed = (renderLed + 1) % 8;
}

#define ANIM_STATE_IDLE       0
#define ANIM_STATE_INITIALIZE 1
#define ANIM_STATE_ANIMATE    2

#define ANIM_PATTERN_COUNT 3

void idleLed(void) {
  static int state = ANIM_STATE_ANIMATE;
  static unsigned long animateUntil = 0;
  static unsigned long nextFrame = 0;
  static int pattern = 0;
  unsigned long now = millis();

  switch (state) {
    case ANIM_STATE_IDLE:
      if (animateUntil < now) {
        //pattern = random(ANIM_PATTERN_COUNT);
        pattern = (pattern + 1) % ANIM_PATTERN_COUNT;
        state = ANIM_STATE_INITIALIZE;
        memset(ledState, HIGH, sizeof(ledState));
      }
      break;
    case ANIM_STATE_INITIALIZE:
      static int tail = 0;
      static int head = 1;
      animateUntil = now + 10000;
      nextFrame = now + 100;
      switch (pattern) {
        case 0:
          ledState[0] = LOW;
          ledState[1] = LOW;
          head = 1;
          tail = 0;
          break;
        case 1:
          ledState[0] = LOW;
          ledState[1] = LOW;
          head = 0;
          tail = 1;
          break;
        case 2:
          break;
      }
      state = ANIM_STATE_ANIMATE;
      break;
    case ANIM_STATE_ANIMATE:
      if (nextFrame < now) { 
        nextFrame = now + 100;
        switch (pattern) { 
          case 0:
            ledState[tail] = HIGH;
            tail = (tail + 1) % 8;
            head = (head + 1) % 8;
            ledState[head] = LOW;
            break;
          case 1:
            ledState[tail] = HIGH;
            tail = (tail + 7) % 8;
            head = (head + 7) % 8;
            ledState[head] = LOW;
            break;
          case 2:
            static int flash = LOW;
            memset(ledState, flash, sizeof(ledState));
            flash = flash == LOW ? HIGH : LOW;
            break;
        }
      }
      if (animateUntil < now) {
        memset(ledState, HIGH, sizeof(ledState));
        ledState[0] = LOW;
        ledState[7] = LOW;
        state = ANIM_STATE_IDLE;
        animateUntil = now + 30000;
      }
      break;
  }
}

int getArmPos(void){
  static int armPos = -1;
  static int lastEnable = HIGH;
  if ((lastEnable == HIGH) && (digitalRead(HALL_GS) == LOW)) { 
    int a_zero_state = digitalRead(HALL1);
    int a_one_state = digitalRead(HALL2);
    int a_two_state = digitalRead(HALL3);
    armPos = (!a_two_state << 2) | (!a_one_state << 1) | (!a_zero_state);
  }
  lastEnable = digitalRead(HALL_GS);
  return armPos;
}

// Constants for the response messages
const byte button[2] = { ADK_COMMAND_BUTTON, 0 };
const byte done[2] = { ADK_COMMAND_MIX_DONE, 0 };

void startMix(int turn_to, unsigned long dispense) {
  int turn_distance = (8 + turn_to - getArmPos()) % 8;
  Serial.print("Turn to distance:");
  Serial.println(turn_distance, DEC);
  if (turn_distance < 4) {
      lastDirection = CW;
  } else {
      lastDirection = CCW;
  }
  if (turn_distance != 0) {
      MotorCtrl(lastDirection, 125);
  }
}

void loop() {
  static int state = STATE_IDLE; 
  static int encountered;
  static unsigned long encounteredAt;
  static int turn_to;
  static unsigned long dispense;
  static int serialRead = 0;

  if (Serial.available() > 0) {
    if (serialRead == 0) {
        turn_to = (Serial.read() - '0');
        if ((turn_to >= 0) && (turn_to < 8)) {
            Serial.print("Turn to valve ");
            Serial.print(char(turn_to));
            turn_to = (turn_to + 2) % 8;
            Serial.print(" and dispense (1/2 oz): ");
            serialRead = 1;
            dispense = 0;
        }
    } else {
        serialRead = Serial.read();
        if ((serialRead >= '0') && (serialRead <= '9')) {
            Serial.print(char(serialRead));
            dispense = dispense * 10 + (serialRead - '0');
        } else {
          Serial.println();
          Serial.print("Dispensing ");
          Serial.print(dispense, DEC);
          Serial.println(" * 1/2 oz");
          closeValves();
          startMix(turn_to, dispense);
          state = STATE_TURN;
          serialRead = 0;
        }
    }
  }
  ledLoop();

  byte msg[6];
  if (acc.isConnected()) {
    if (buttonState) {
      Serial.println("Button pressed.");
      acc.write((void*)button, sizeof(button));
      buttonState = 0;
    }
    // Check for messages from android.
    int len = acc.read(msg, sizeof(msg), 1);
    if (len > 0) {
      Serial.println(len, DEC);
      for(int i=0; i < len; i++) {
        Serial.print(msg[i], HEX);
        Serial.print(" ");
      }
      Serial.println();
      //TODO: apparently there's a second byte there? 
      // I should probably do something with it
      // msg[1] == length of message
      switch(msg[0]) {
          case COMMAND_MIX:
              closeValves();
              turn_to = (msg[2] + HALL_OFFSET) % 8;
              dispense = msg[3];
              startMix(turn_to, dispense);
              state = STATE_TURN;
              break;
      }
    }
  }
  static unsigned long dispenseUntil;
  static int led1;
  static int led2;
  unsigned long now = millis();
  switch (state) {

    case STATE_IDLE:
      idleLed();
      break;
    case STATE_TURN:
      static int lastPos = -1;
      int pos;
      pos = getArmPos();
      if (pos != lastPos) {
          led1 = (pos - 3 + 8) % 8;
          led2 = (pos - 2 + 8) % 8;
          lastPos = pos;
          memset(ledState, HIGH, sizeof(ledState));
          ledState[led1] = LOW;
          ledState[led2] = LOW;
      }
      if (pos == turn_to) {
        MotorCtrl(lastDirection, 0);
        dispenseUntil = now + (3000l * dispense);
        memset(ledState, HIGH, sizeof(ledState));
        openValve((turn_to - HALL_OFFSET) % 8);
        state = STATE_PAUSE;
      }
      break;
    case STATE_PAUSE:
      static unsigned long nextFrame = 0;
      static int flash = LOW;
      if (nextFrame < now) {
        nextFrame = now + 200;
        flash = flash == LOW ? HIGH : LOW;
        ledState[led1] = flash;
        ledState[led2] = flash;
      }
      if (dispenseUntil < now) {
        closeValves();
        state = STATE_IDLE;
        acc.write((void*)done, sizeof(done));
      }
      break;
  }
}

// vim: filetype=cpp
